﻿using System.Numerics;

/*测试数据
 * 测试数据
 
//ENU->XYZ
//U轴->E轴

 E 12490 N 12280 U 2303.02
 E 12490 N 12280 U 3347.6
 E 12642.4 N 12280 U 3500
 E 13835.19 N 12280 U 3500
//N轴->E轴

 E 13939.96 N 12384.77 U 3500
 E 13939.96 N 14147.6 U 3500
 E 13787.56 N 14300 U 3500
 E 12642.52 N 14300 U 3500
//E轴->U轴

 E 13787.56 N 14300 U 3500
 E 12642.52 N 14300 U 3500
 E 12490.12 N 14300 U 3347.6
 E 12490.12 N 14300 U 2495.07

//N轴->U轴

 E 12490.12 N 16104.77 U 300
 E 12490.12 N 16349.6 U 300
 E 12490.12 N 16502 U 452.4
 E 12490.12 N 16502 U 4785.57
 
//N轴->E轴

 E 13939.96 N 14147.6 U 3500
 E 13939.96 N 12384.77 U 3500
 E 13787.56 N 14300 U 3500
 E 12642.52 N 14300 U 3500
 q apos

//歪着
 E 3854.58 N 8850 U 5521.2
 E 3854.58 N 8850 U 1722.38
 E 3566.749 N 8950.282 U 1417.58
 E 3137.831 N 9099.718 U 1417.58

 */
namespace GeometryLib
{
    public class Tools3
    {

        public static bool GetIntersectionXYZ(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            result = new Point3D();
            //四点是否共面,不共面一定没有交点
            var res = PlaneCalculator.FourPointPlane(pt1, pt2, pt3, pt4);
            //var res = FourPointsCoplanar(pt1,pt2, pt3, pt4);
            if (!res)
            {
#if DEBUG
                Console.WriteLine("四点不共面");
#endif
                return false;
            }

            Vector3 v1 = new Vector3((float)(pt2.X - pt1.X), (float)(pt2.Y - pt1.Y), (float)(pt2.Z - pt1.Z));
            Vector3 v2 = new Vector3((float)(pt4.X - pt3.X), (float)(pt4.Y - pt3.Y), (float)(pt4.Z - pt3.Z));
            Point3D intersection = new Point3D();
            bool resultDs = false;

            if (((v1.Y == 0 && v1.X == 0) && (v2.X == 0 && v2.Z == 0)) ||
                ((v1.X == 0 && v1.Z == 0) && (v2.Y == 0 && v2.X == 0)))     //NU Y<--->Z轴 Z轴竖直
            {
                resultDs = GetIntersectionYZ(pt1, pt2, pt3, pt4, out intersection);
            }
            else if (((v1.Y == 0 && v1.X == 0) && (v2.Y == 0 && v2.Z == 0)) ||
                ((v1.Y == 0 && v1.Z == 0) && (v2.Y == 0 && v2.X == 0)))     //EN X<--->Z轴 Z轴竖直   
            {
                resultDs = GetIntersectionXZ(pt1, pt2, pt3, pt4, out intersection);
            }
            else if (((v1.Y == 0 && v1.Z == 0) && (v2.X == 0 && v2.Z == 0)) ||
                ((v1.X == 0 && v1.Z == 0) && (v2.Y == 0 && v2.Z == 0)))     //EU X<--->Y轴
            {
                resultDs = GetIntersectionXY(pt1, pt2, pt3, pt4, out intersection);
            }//Z轴 XY面
            else if (((v1.Y == 0 && v1.X == 0) && (v2.Z == 0)) ||
                    ((v2.Y == 0 && v2.X == 0) && (v1.Z == 0)))
            {
                resultDs = GetIntersectionZ2XY(pt1, pt2, pt3, pt4, out intersection);

            }//X轴 YZ面
            else if (((v1.Y == 0 && v1.Z == 0) && (v2.X == 0)) ||
                    ((v2.Y == 0 && v2.Z == 0) && (v1.X == 0)))
            {
                resultDs = GetIntersectionX2YZ(pt1, pt2, pt3, pt4, out intersection);
            }//Y轴 XZ面
            else if (((v1.X == 0 && v1.Z == 0) && (v2.Y == 0)) ||
                    ((v2.X == 0 && v2.Z == 0) && (v1.Y == 0)))
            {
                resultDs = GetIntersectionY2XZ(pt1, pt2, pt3, pt4, out intersection);

            }
            else if (((v1.X == 0 && v1.Y == 0) && (v2.X != 0 && v2.Y != 0)) ||
                    ((v2.X == 0 && v2.Y == 0) && (v1.X != 0 && v1.Y != 0))) //Z2XY_I Z与 XY斜面
            {
                resultDs = GetIntersectionZ2XY_I(pt1,pt2, pt3, pt4, out intersection);
            }
            else if (((v1.X == 0 && v1.Z == 0) && (v2.X != 0 && v2.Z != 0)) ||
                    ((v2.X == 0 && v2.Z == 0) && (v1.X != 0 && v1.Z != 0))) //Y2XZ_I Y与 XZ斜面
            {
                resultDs = GetIntersectionY2XZ_I(pt1, pt2, pt3, pt4, out intersection);
            }
            else if (((v1.Y == 0 && v1.Z == 0) && (v2.Y != 0 && v2.Z != 0)) ||
                    ((v2.Y == 0 && v2.Z == 0) && (v1.Y != 0 && v1.Z != 0))) //X2YZ_I X与 YZ斜面
            {
                resultDs = GetIntersectionX2YZ_I(pt1, pt2, pt3, pt4, out intersection);
            }
            else
            {
                resultDs = FourPointsIntersection(pt1,pt2,pt3,pt4,out intersection);
            }
            result = intersection;
            return resultDs;
        }
        #region 轴<---->斜平面
        /// <summary>
        /// 平行Z轴与倾斜XY面相交
        /// </summary>
        /// <returns></returns>
        public static bool GetIntersectionZ2XY_I(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            Vector3 v1 = new Vector3((float)(pt2.X - pt1.X), (float)(pt2.Y - pt1.Y), (float)(pt2.Z - pt1.Z));
            Vector3 v2 = new Vector3((float)(pt4.X - pt3.X), (float)(pt4.Y - pt3.Y), (float)(pt4.Z - pt3.Z));

            result = new Point3D();
            double t=0.0;

            if ((v1.X == 0 && v1.Y == 0))
            {
                //Pt3,Pt4
                //(y-y2)/ (y1-y2) = (x-x2)/ (x1-x2)
                if (v2.Y != 0 && v2.X != 0)
                {
                    //L = p + tV
                    //L1 = P3+ t*V2
                    //t = (L1.y-P3.y)/V2.Y
                    t = (pt1.Y - pt3.Y) / v2.Y;
                    var _z = pt3.Z + t* v2.Z;

                    result = new Point3D(pt1.X, pt1.Y, _z);
                    return true;
                }
                else if (v2.Y != 0 && v2.Z != 0)
                {
                    //y = y2+ (z-z2)*(y1-y2)/(z1-z2)
                    t = (pt1.Y - pt3.Y) / v2.Y;
                    var _z = pt3.Z + t * v2.Z;

                    result = new Point3D(pt1.X, pt1.Y, _z);
                    return true;

                }
                else if (v2.X != 0 && v2.Z != 0)
                {
                    //x = x2+ (z-z2)*(x1-x2)/(z1-z2)
                    t = (pt1.X - pt3.X) / v2.X;
                    var _z = pt3.Z + t * v2.Z;

                    result = new Point3D(pt1.X, pt1.Y, _z);
                    return true;
                }


            }
            else if (v2.X == 0 && v2.Y == 0)
            {
                if (v1.X != 0 && v1.Y != 0)
                {
                    t = (pt3.Y - pt1.Y) / v1.Y;
                    var _z = pt1.Z + t * v1.Z;
                    result = new Point3D(pt1.X, pt1.Y, _z);
                    return true;
                }
                else if (v1.Y != 0 && v1.Z != 0)
                {
                    t = (pt3.Y - pt1.Y) / v1.Y;
                    var _z = pt1.Z + t * v1.Z;
                    result = new Point3D(pt1.X, pt1.Y, _z);
                    return true;

                }
                else if (v1.Z != 0 && v1.X != 0)
                {
                    t = (pt3.X - pt1.X) / v1.X;
                    var _z = pt1.Z + t * v1.Z;
                    result = new Point3D(pt1.X, pt1.Y, _z);
                    return true;

                }
            }
            return false;
        }
        /// <summary>
        /// 平行X轴与倾斜YZ面相交
        /// </summary>
        /// <param name="pt1"></param>
        /// <param name="pt2"></param>
        /// <param name="pt3"></param>
        /// <param name="pt4"></param>
        /// <param name="result"></param>
        /// <returns></returns>
        public static bool GetIntersectionX2YZ_I(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            result = new Point3D();
            Vector3 v1 = new Vector3((float)(pt2.X - pt1.X), (float)(pt2.Y - pt1.Y), (float)(pt2.Z - pt1.Z));
            Vector3 v2 = new Vector3((float)(pt4.X - pt3.X), (float)(pt4.Y - pt3.Y), (float)(pt4.Z - pt3.Z));

            double t = 0.0;

            if ((v1.Z == 0 && v1.Y == 0))
            {
                //Pt3,Pt4
                //(y-y2)/ (y1-y2) = (x-x2)/ (x1-x2)
                if (v2.Y != 0 && v2.X != 0)
                {
                    //L = p + tV
                    //L1 = P3+ t*V2
                    //t = (L1.y-P3.y)/V2.Y
                    t = (pt1.Y - pt3.Y) / v2.Y;
                    var _x = pt3.X + t * v2.X;

                    result = new Point3D(_x, pt1.Y, pt1.X);
                    return true;
                }
                else if (v2.Y != 0 && v2.Z != 0)
                {
                    //y = y2+ (z-z2)*(y1-y2)/(z1-z2)
                    t = (pt1.Y - pt3.Y) / v2.Y;
                    var _x = pt3.X + t * v2.X;

                    result = new Point3D(_x, pt1.Y, pt1.X);
                    return true;

                }
                else if (v2.X != 0 && v2.Z != 0)
                {
                    //x = x2+ (z-z2)*(x1-x2)/(z1-z2)
                    t = (pt1.Z - pt3.Z) / v2.Z;
                    var _z = pt3.X + t * v2.X;

                    result = new Point3D(_z ,pt1.Y, pt1.Z);
                    return true;
                }


            }
            else if (v2.Z == 0 && v2.Y == 0)
            {
                if (v1.X != 0 && v1.Y != 0)
                {
                    t = (pt3.Y - pt1.Y) / v1.Y;
                    var _x = pt1.X + t * v1.X;
                    result = new Point3D(_x,pt3.Y,  pt3.Z);
                    return true;
                }
                else if (v1.Y != 0 && v1.Z != 0)
                {
                    t = (pt3.Y - pt1.Y) / v1.Y;
                    var _x = pt1.X + t * v1.X;
                    result = new Point3D(_x, pt3.Y, pt3.Z);
                    return true;

                }
                else if (v1.Z != 0 && v1.X != 0)
                {
                    t = (pt3.Z - pt1.Z) / v1.Z;
                    var _x = pt1.X + t * v1.X;
                    result = new Point3D(_x,pt1.Y,  pt1.Z);
                    return true;

                }
            }
            return false;



            return false;
        }
        /// <summary>
        /// 平行Y轴与倾斜XZ面相交
        /// </summary>
        /// <param name="pt1"></param>
        /// <param name="pt2"></param>
        /// <param name="pt3"></param>
        /// <param name="pt4"></param>
        /// <param name="result"></param>
        /// <returns></returns>
        public static bool GetIntersectionY2XZ_I(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            Vector3 v1 = new Vector3((float)(pt2.X - pt1.X), (float)(pt2.Y - pt1.Y), (float)(pt2.Z - pt1.Z));
            Vector3 v2 = new Vector3((float)(pt4.X - pt3.X), (float)(pt4.Y - pt3.Y), (float)(pt4.Z - pt3.Z));

            result = new Point3D();
            double t = 0.0;

            if ((v1.X == 0 && v1.Z == 0))
            {
                //Pt3,Pt4
                //(y-y2)/ (y1-y2) = (x-x2)/ (x1-x2)
                if (v2.Y != 0 && v2.X != 0)
                {
                    //L = p + tV
                    //L1 = P3+ t*V2
                    //t = (L1.y-P3.y)/V2.Y
                    t = (pt1.X - pt3.X) / v2.X;
                    var _y = pt3.Y + t * v2.Y;

                    result = new Point3D(pt1.X,_y, pt1.Z);
                    return true;
                }
                else if (v2.Y != 0 && v2.Z != 0)
                {
                    //y = y2+ (z-z2)*(y1-y2)/(z1-z2)
                    t = (pt1.Z - pt3.Z) / v2.Z;
                    var _y = pt3.Y + t * v2.Y;

                    result = new Point3D(pt1.X, _y, pt1.Z);
                    return true;
                }
                else if (v2.X != 0 && v2.Z != 0)
                {
                    //x = x2+ (z-z2)*(x1-x2)/(z1-z2)
                    t = (pt1.X - pt3.X) / v2.X;
                    var _y = pt3.Y + t * v2.Y;

                    result = new Point3D(pt1.X,_y, pt1.Z);
                    return true;
                }
            }
            else if (v2.X == 0 && v2.Z == 0)
            {
                if (v1.X != 0 && v1.Y != 0)
                {
                    t = (pt3.X - pt1.X) / v1.X;
                    var _y = pt1.Y + t * v1.Y;
                    result = new Point3D(pt1.X, _y, pt1.Z);
                    return true;
                }
                else if (v1.Y != 0 && v1.Z != 0)
                {
                    t = (pt3.Z - pt1.Z) / v1.Z;
                    var _y = pt1.Y + t * v1.Y;
                    result = new Point3D(pt1.X, _y, pt1.Z);
                    return true;

                }
                else if (v1.Z != 0 && v1.X != 0)
                {
                    t = (pt3.X - pt1.X) / v1.X;
                    var _y = pt1.Y + t * v1.Y;
                    result = new Point3D(pt1.X, _y, pt1.Z);
                    return true;

                }
            }
            return false;
        }
        #endregion
        #region 轴<---->平面 (垂直关系)
        public static bool GetIntersectionZ2XY(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            result = new Point3D();

            //z(1)->xy(2)
            if (pt3.Z == pt4.Z)
            {
                result = new Point3D(pt1.X, pt1.Y, pt2.Z);
            }
            else if (pt1.Z == pt2.Z)
            {
                result = new Point3D(pt2.X, pt2.Y, pt1.Z);
            }

            return true;
        }
        public static bool GetIntersectionX2YZ(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            result = new Point3D();

            //z(1)->xy(2)
            if (pt3.X == pt4.X)
            {
                result = new Point3D(pt1.X, pt1.Y, pt3.Z);
            }
            else if (pt1.X == pt2.X)
            {
                result = new Point3D(pt1.X, pt3.Y, pt3.Z);
            }

            return true;
        }
        public static bool GetIntersectionY2XZ(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            result = new Point3D();

            //z(1)->xy(2)
            if (pt3.Y == pt4.Y)
            {
                result = new Point3D(pt3.X, pt1.Y, pt1.Z);
            }
            else if (pt1.Y == pt2.Y)
            {
                result = new Point3D(pt3.X, pt1.Y, pt3.Z);
            }

            return true;
        }

        #endregion
        #region 轴<---->轴
        public static bool GetIntersectionXY(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            result = new Point3D();

            result = new Point3D();
            Vector3 v1 = new Vector3((float)(pt2.X - pt1.X), (float)(pt2.Y - pt1.Y), (float)(pt2.Z - pt1.Z));
            Vector3 v2 = new Vector3((float)(pt4.X - pt3.X), (float)(pt4.Y - pt3.Y), (float)(pt4.Z - pt3.Z));
            if (v1.Z == 0 && v2.Z == 0 && Math.Abs(pt1.Z - pt3.Z) > 3)//3mm 间隙
            {
                return false;
            }
            double t;
            double s;
            //Y,Z 联立
            //pt1.Y + t * v1.Y = pt3.Y + s * v2.Y;
            //pt1.X + t * v1.X = pt3.X + s * v2.X;

            //ax+by = c
            //v1.Y * t  -v2.Y * s  = pt3.Y-pt1.Y;
            //v1.X * t  -v2.X * s  = pt3.X-pt1.X;
            //解 t,s
            //系数 v1.Y,-v2.Y,pt3.Y-pt1.Y,v1.X,-V2.X,pt3.X - pt1.X

            var calaResult = SolveLinearEquations(v1.Y, -v2.Y, pt3.Y - pt1.Y, v1.X, -v2.X, pt3.X - pt1.X, out t, out s);
            result = new Point3D(pt1.X + t * v1.X, pt1.Y + t * v1.Y, pt1.Z);
            return calaResult;
        }
        public static bool GetIntersectionXZ(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            result = new Point3D();

            result = new Point3D();
            Vector3 v1 = new Vector3((float)(pt2.X - pt1.X), (float)(pt2.Y - pt1.Y), (float)(pt2.Z - pt1.Z));
            Vector3 v2 = new Vector3((float)(pt4.X - pt3.X), (float)(pt4.Y - pt3.Y), (float)(pt4.Z - pt3.Z));
            if (v1.Y == 0 && v2.Y == 0 && Math.Abs(pt1.Y - pt3.Y) > 3)//3mm 间隙
            {
                return false;
            }
            double t;
            double s;
            //Y,Z 联立
            //pt1.X + t * v1.X = pt3.X + s * v2.X;
            //pt1.Z + t * v1.Z = pt3.Z + s * v2.Z;

            //ax+by = c
            //v1.X * t  -v2.X * s  = pt3.X-pt1.X;
            //v1.Z * t  -v2.Z * s  = pt3.Z-pt1.Z;
            //解 t,s
            //系数 v1.Y,-v2.Y,pt3.Y-pt1.Y,v1.Z,-V2.Z,pt3.Z - pt1.Z

            var calaResult = SolveLinearEquations(v1.X, -v2.X, pt3.X - pt1.X, v1.Z, -v2.Z, pt3.Z - pt1.Z, out t, out s);
            result = new Point3D(pt1.X + t * v1.X, pt1.Y, pt1.Z + t * v1.Z);
            return calaResult;
        }
        public static bool GetIntersectionYZ(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D result)
        {
            result = new Point3D();
            Vector3 v1 = new Vector3((float)(pt2.X - pt1.X), (float)(pt2.Y - pt1.Y), (float)(pt2.Z - pt1.Z));
            Vector3 v2 = new Vector3((float)(pt4.X - pt3.X), (float)(pt4.Y - pt3.Y), (float)(pt4.Z - pt3.Z));
            if (v1.X == 0 && v2.X == 0 && Math.Abs(pt1.X - pt3.X) > 3)//3mm 间隙
            {
                return false;
            }

            double t;
            double s;

            //Y,Z 联立
            //pt1.Y + t * v1.Y = pt3.Y + s * v2.Y;
            //pt1.Z + t * v1.Z = pt3.Z + s * v2.Z;

            //ax+by = c
            //v1.Y * t  -v2.Y * s  = pt3.Y-pt1.Y;
            //v1.Z * t  -v2.Z * s  = pt3.Z-pt1.Z;
            //解 t,s
            //系数 v1.Y,-v2.Y,pt3.Y-pt1.Y,v1.Z,-V2.Z,pt3.Z - pt1.Z


            var calaResult = SolveLinearEquations(v1.Y, -v2.Y, pt3.Y - pt1.Y, v1.Z, -v2.Z, pt3.Z - pt1.Z, out t, out s);
            result = new Point3D(pt1.X, pt1.Y + t * v1.Y, pt1.Z + t * v1.Z);
            return calaResult;
        }

        #endregion

        //  public static bool Solve(
        //double a1, double b1, double c1,
        //double a2, double b2, double c2,out PointF interPoint)
        //  {
        //      interPoint = new PointF();
        //      // 构建主矩阵 
        //      var mainMatrix = Matrix<double>.Build.DenseOfArray(new[,] {
        //      { a1, b1 },
        //      { a2, b2 }
        //  });

        //      // 计算主行列式
        //      double D = mainMatrix.Determinant();
        //      if (Math.Abs(D) < 1e-12)
        //          return false;

        //      // 构建替换矩阵 
        //      var matrixX = Matrix<double>.Build.DenseOfArray(new[,] {
        //      { c1, b1 },
        //      { c2, b2 }
        //  });
        //      var matrixY = Matrix<double>.Build.DenseOfArray(new[,] {
        //      { a1, c1 },
        //      { a2, c2 }
        //  });

        //      // 计算行列式
        //      double Dx = matrixX.Determinant();
        //      double Dy = matrixY.Determinant();
        //      interPoint = new PointF((float)(Dx/D),(float)(Dy/D));
        //      return true;
        //      //return (Dx / D, Dy / D);
        //  }


        /*
         *   适用于二元一次方程组形式：
         *   {
	     *       a1x+b1y=c1
	     *       a2x+b2y=c2
         *   }
         *
         *   克莱姆法则（Cramer's Rule）
         *   对应的矩阵形式为 
         *   AX=B，其中：
         *
         *   A=|a1 b1|
         *     |a2 b2|
         *
         *   B=|c1|
         *     |c2|
         *
         *   X=|x|
         *     |y|
         *
         *   代码实现：
         *  SolveLinearEquations(a1,b1,c1,a2,b2,c2)
         */
        // 输入系数：2x+3y=5, 4x+y=6  SolveLinearEquations(2,3,5,4,1,6);

        /// <summary>
        /// 两个二元一次方程求解
        /// </summary>
        /// <param name="a1">x系数</param>
        /// <param name="b1">y系数</param>
        /// <param name="c1">常数</param>
        /// <param name="a2">x系数</param>
        /// <param name="b2">y系数</param>
        /// <param name="c2">常数</param>
        /// <param name="x">x值</param>
        /// <param name="y">y值</param>
        /// <returns></returns>
        public static bool SolveLinearEquations(double a1, double b1, double c1, double a2, double b2, double c2,out double x,out double y)
        {        
            // 输入系数：2x+3y=5, 4x+y=6  SolveLinearEquations(2,3,5,4,1,6);
            x = 0.0;
            y = 0.0;
            double det = a1 * b2 - a2 * b1; // 系数矩阵行列式
            if (det == 0)
            {
                return false;
            }
            x = (c1 * b2 - c2 * b1) / det;
            y = (a1 * c2 - a2 * c1) / det;
            return true;
        }


        /// <summary>
        /// 判断四点是否共面
        /// </summary>
        /// <param name="pointA">第一点(X,Y,Z)</param>
        /// <param name="pointB">第二点(X,Y,Z)</param>
        /// <param name="pointC">第三点(X,Y,Z)</param>
        /// <param name="pointD">第四点(X,Y,Z)</param>
        /// <param name="epsilon">冗余阈值</param>
        /// <returns>true 共面[共线]；false 不共面</returns>
        public static bool FourPointsCoplanar(Point3D pointA, Point3D pointB, Point3D pointC, Point3D pointD)
        {
            double epsilon = 1e-7;
            Vector3 AB = new Vector3((float)(pointB.X - pointA.X), (float)(pointB.Y - pointA.Y), (float)(pointB.Z - pointA.Z));
            Vector3 AC = new Vector3((float)(pointC.X - pointA.X), (float)(pointC.Y - pointA.Y), (float)(pointC.Z - pointA.Z));
            Vector3 AD = new Vector3((float)(pointD.X - pointA.X), (float)(pointD.Y - pointA.Y), (float)(pointD.Z - pointA.Z));

            Vector3 cross0 = Vector3.Cross(AC, AD);
            double mixedProduct0 = Vector3.Dot(AB, cross0);

            Vector3 cross1 = Vector3.Cross(AC, AB);
            double mixedProduct1 = Vector3.Dot(AD, cross1);

            Vector3 cross2 = Vector3.Cross(AB, AD);
            double mixedProduct2 = Vector3.Dot(AC, cross2);
            Console.WriteLine($"{mixedProduct0};{mixedProduct1};{mixedProduct2}");

            return (Math.Abs(mixedProduct0) < epsilon) || (Math.Abs(mixedProduct1) < epsilon) || (Math.Abs(mixedProduct2)< epsilon);

            Vector3 ABUnit = AB.ToUnitVector();
            Vector3 ACUnit = AC.ToUnitVector();
            Vector3 ADUnit = AD.ToUnitVector();

            Vector3 cross = Vector3.Cross(ACUnit, ADUnit);
            double mixedProduct = Vector3.Dot(ABUnit, cross);

            return Math.Abs(mixedProduct) < epsilon;
        }


        public static bool FourPointsIntersection(Point3D pt1, Point3D pt2, Point3D pt3, Point3D pt4, out Point3D intersection)
        {
            double epsilon = 1E-3;

            intersection = default(Point3D);
            // 直线L1的点和方向向量
            Point3D line1Start = pt1;
            Vector3 v1 = new Vector3((float)(pt1.X - pt2.X), (float)(pt1.Y - pt2.Y), (float)(pt1.Z - pt2.Z));

            // 直线L2的点和方向向量
            Point3D line2Start = pt3;
            Vector3 v2 = new Vector3((float)(pt3.X - pt4.X), (float)(pt3.Y - pt4.Y), (float)(pt3.Z - pt4.Z));

            double t = 0.0;
            double s = 0.0;
            double z1 = 0.0;
            double z2 = 0.0;

            //X Y 组成方程
            // 构造前两个方程的系数矩阵
            float[,] A = {
                            { v1.X, -v2.X },
                            { v1.Y, -v2.Y }
                        };

            double [] B = {
                            pt3.X - pt1.X,
                            pt3.Y - pt1.Y
                        };

            // 计算行列式
            float det = A[0, 0] * A[1, 1] - A[0, 1] * A[1, 0];

            if (Math.Abs(det) < epsilon)
            {
                 A = new float[,] {
                                        { v1.X, -v2.X },
                                        { v1.Z, -v2.Z }
                                    };

                B = new double[] {pt3.X - pt1.X,pt3.Z - pt1.Z};

                det = A[0, 0] * A[1, 1] - A[0, 1] * A[1, 0];
                if (Math.Abs(det) < epsilon)
                {
                    A = new float[,] {
                                        { v1.Y, -v2.Y },
                                        { v1.Z, -v2.Z }
                                    };
                    B = new double[] { pt3.Y - pt1.Y, pt3.Z - pt1.Z };

                    det = A[0, 0] * A[1, 1] - A[0, 1] * A[1, 0];
                    if (Math.Abs(det) < epsilon)
                    {
                        return false;
                    }                // 克莱姆法则求解t和s
                    t = (A[1, 1] * B[0] - A[0, 1] * B[1]) / det;
                    s = (A[0, 0] * B[1] - A[1, 0] * B[0]) / det;

                    // 验证第三个分量
                    z1 = line1Start.X + t * v1.X;
                    z2 = line2Start.X + s * v2.X;
                    if (Math.Abs(z1 - z2) > epsilon)
                        return false; // 异面直线

                    intersection = line1Start.AddVector3(v1.ToScalar(t));
                    return true;
                }
                // 克莱姆法则求解t和s
                t = (A[1, 1] * B[0] - A[0, 1] * B[1]) / det;
                s = (A[0, 0] * B[1] - A[1, 0] * B[0]) / det;

                // 验证第三个分量
                z1 = line1Start.Y + t * v1.Y;
                z2 = line2Start.Y + s * v2.Y;
                if (Math.Abs(z1 - z2) > epsilon)
                    return false; // 异面直线

                intersection = line1Start.AddVector3(v1.ToScalar(t));

                return true;
            }

            // 克莱姆法则求解t和s
            t = (A[1, 1] * B[0] - A[0, 1] * B[1]) / det;
            s = (A[0, 0] * B[1] - A[1, 0] * B[0]) / det;

            // 验证第三个分量
            z1 = line1Start.Z + t * v1.Z;
            z2 = line2Start.Z + s * v2.Z;
            if (Math.Abs(z1 - z2) > epsilon)
                return false; // 异面直线

            intersection = line1Start.AddVector3(v1.ToScalar(t));


            return true;
        }

    }
}
